#ifndef __SPARSE_GRAPH_H_INCLUDE__
#define __SPARSE_GRAPH_H_INCLUDE__

#include <vector>
#include <list>
#include <cassert>

template <class node_type, class edge_type>
class SparseGraph
{
public:
	typedef edge_type EdgeType;
	typedef node_type NodeType;

	typedef std::vector<node_type> NodeVector;
	typedef std::list<edge_type> EdgeList;
	typedef std::vector<EdgeList> EdgeListVector;

private:
	NodeVector m_Nodes;
	EdgeListVector m_Edges;
	bool m_isDigraph;
	int m_iNextNodeIndex;
	bool UniqueEdge(int from, int to)const;
	void CullInvalidEdges();

public:
	SparseGraph(bool isDigraph):m_isDigraph(isDigraph), m_iNextNodeIndex(0){}

	const NodeType& getNode(int idx)const;
	const EdgeType& getEdge(int source, int dest)const;
	NodeType& getNode(int index);
	EdgeType& getEdge(int source, int dest);

	int getNextFreeNodeIndex()const{return m_iNextNodeIndex;}

	int addNode(NodeType node);
	void removeNode(int node);

	void addEdge(EdgeType edge);
	void removeEdge(int src, int dest);

	void setEdgeCost(int src, int dest, double cost);

	int getNodeCount()const{
		return m_Nodes.size();
	}

	int getActiveNodeCount()const{
		int count = 0;
		for(unsigned int n=0; n<m_Nodes.size(); ++n)
			if(m_Nodes[n].getIndex() != invalid_node_index) 
				++count;
	}

	int getEdgeCount()const{
		int edgeCount = 0;
		for(EdgeListVector::const_iterator currentEdge = m_Edges.begin();
			currentEdge != m_Edges.end();
			++currentEdge){
				edgeCount += currentEdge->size();
		}

		return edgeCount;
	}

	bool isDigraph()const{return m_isDigraph;}
	bool isEmpty()const{return m_Nodes.empty();}
	bool isNodePresent(int nd)const;
	bool isEdgePresent(int from, int to)const;
	void Clear(){m_iNextNodeIndex = 0; m_Nodes.clear(); m_Edges.clear();}

	void RemoveEdges()
	{
		for (EdgeListVector::iterator it = m_Edges.begin(); it != m_Edges.end(); ++it)
		{
			it->clear();
		}
	}



	//non const class used to iterate through all the edges connected to a specific node. 
	class EdgeIterator
	{
	private:                                                                

		typename EdgeList::iterator         curEdge;

		SparseGraph<node_type, edge_type>&  G;

		const int                           NodeIndex;

	public:

		EdgeIterator(SparseGraph<node_type, edge_type>& graph,
			int                                node): G(graph),
			NodeIndex(node)
		{
			/* we don't need to check for an invalid node index since if the node is
			invalid there will be no associated edges
			*/

			curEdge = G.m_Edges[NodeIndex].begin();
		}

		EdgeType*  begin()
		{        
			curEdge = G.m_Edges[NodeIndex].begin();

			return &(*curEdge);
		}

		EdgeType*  next()
		{
			++curEdge;

			if (end()) return NULL;

			return &(*curEdge);

		}

		//return true if we are at the end of the edge list
		bool end()
		{
			return (curEdge == G.m_Edges[NodeIndex].end());
		}
	};

	friend class EdgeIterator;

	//const class used to iterate through all the edges connected to a specific node. 
	class ConstEdgeIterator
	{
	private:                                                                

		typename EdgeList::const_iterator        curEdge;

		const SparseGraph<node_type, edge_type>& G;

		const int                                NodeIndex;

	public:

		ConstEdgeIterator(const SparseGraph<node_type, edge_type>& graph,
			int                           node): G(graph),
			NodeIndex(node)
		{
			/* we don't need to check for an invalid node index since if the node is
			invalid there will be no associated edges
			*/

			curEdge = G.m_Edges[NodeIndex].begin();
		}

		const EdgeType*  begin()
		{        
			curEdge = G.m_Edges[NodeIndex].begin();

			return &(*curEdge);
		}

		const EdgeType*  next()
		{
			++curEdge;

			if(end())
			{
				return NULL;
			}
			else
			{
				return &(*curEdge);
			}

		}

		//return true if we are at the end of the edge list
		bool end()
		{
			return (curEdge == G.m_Edges[NodeIndex].end());
		}
	};

	friend class ConstEdgeIterator;

	//non const class used to iterate through the nodes in the graph
	class NodeIterator
	{
	private:

		typename NodeVector::iterator         curNode;

		SparseGraph<node_type, edge_type>&    G;

		//if a graph node is removed, it is not removed from the 
		//vector of nodes (because that would mean changing all the indices of 
		//all the nodes that have a higher index). This method takes a node
		//iterator as a parameter and assigns the next valid element to it.
		void GetNextValidNode(typename NodeVector::iterator& it)
		{
			if ( curNode == G.m_Nodes.end() || it->getIndex() != invalid_node_index) return;

			while ( (it->getIndex() == invalid_node_index) )
			{
				++it;

				if (curNode == G.m_Nodes.end()) break;
			}
		}

	public:

		NodeIterator(SparseGraph<node_type, edge_type> &graph):G(graph)
		{
			curNode = G.m_Nodes.begin();
		}


		node_type* begin()
		{      
			curNode = G.m_Nodes.begin();

			GetNextValidNode(curNode);

			return &(*curNode);
		}

		node_type* next()
		{
			++curNode;

			if (end()) return NULL;

			GetNextValidNode(curNode);

			return &(*curNode);
		}

		bool end()
		{
			return (curNode == G.m_Nodes.end());
		}
	};


	friend class NodeIterator;

	//const class used to iterate through the nodes in the graph
	class ConstNodeIterator
	{
	private:

		typename NodeVector::const_iterator			curNode;

		const SparseGraph<node_type, edge_type>&      G;

		//if a graph node is removed or switched off, it is not removed from the 
		//vector of nodes (because that would mean changing all the indices of 
		//all the nodes that have a higher index. This method takes a node
		//iterator as a parameter and assigns the next valid element to it.
		void GetNextValidNode(typename NodeVector::const_iterator& it)
		{
			if ( curNode == G.m_Nodes.end() || it->Index() != invalid_node_index) return;

			while ( (it->Index() == invalid_node_index) )
			{
				++it;

				if (curNode == G.m_Nodes.end()) break;
			}
		}

	public:

		ConstNodeIterator(const SparseGraph<node_type, edge_type> &graph):G(graph)
		{
			curNode = G.m_Nodes.begin();
		}


		const node_type* begin()
		{      
			curNode = G.m_Nodes.begin();

			GetNextValidNode(curNode);

			return &(*curNode);
		}

		const node_type* next()
		{
			++curNode;

			if (end())
			{
				return NULL;
			}
			else
			{
				GetNextValidNode(curNode);

				return &(*curNode);
			}
		}

		bool end()
		{
			return (curNode == G.m_Nodes.end());
		}
	};

	friend class ConstNodeIterator;
};

//--------------------------- isNodePresent --------------------------------
//
//  returns true if a node with the given index is present in the graph
//--------------------------------------------------------------------------
template <class node_type, class edge_type>
bool SparseGraph<node_type, edge_type>::isNodePresent(int nd)const
{
	if ((nd >= (int)m_Nodes.size() || (m_Nodes[nd].getIndex() == invalid_node_index)))
	{
		return false;
	}
	else return true;
}

//--------------------------- isEdgePresent --------------------------------
//
//  returns true if an edge with the given from/to is present in the graph
//--------------------------------------------------------------------------
template <class node_type, class edge_type>
bool SparseGraph<node_type, edge_type>::isEdgePresent(int from, int to)const
{
	if (isNodePresent(from) && isNodePresent(from))
	{
		for (EdgeList::const_iterator curEdge = m_Edges[from].begin();
			curEdge != m_Edges[from].end();
			++curEdge)
		{
			if (curEdge->getDestination() == to) return true;
		}

		return false;
	}
	else return false;
}
//------------------------------ GetNode -------------------------------------
//
//  const and non const methods for obtaining a reference to a specific node
//----------------------------------------------------------------------------
template <class node_type, class edge_type>
const node_type&  SparseGraph<node_type, edge_type>::getNode(int idx)const
{
	assert( (idx < (int)m_Nodes.size()) &&
		(idx >=0)              &&
		"<SparseGraph::GetNode>: invalid index");

	return m_Nodes[idx];
}

//non const version
template <class node_type, class edge_type>
node_type&  SparseGraph<node_type, edge_type>::getNode(int idx)
{
	assert( (idx < (int)m_Nodes.size()) &&
		(idx >=0)             &&
		"<SparseGraph::GetNode>: invalid index");

	return m_Nodes[idx];
}

//------------------------------ GetEdge -------------------------------------
//
//  const and non const methods for obtaining a reference to a specific edge
//----------------------------------------------------------------------------
template <class node_type, class edge_type>
const edge_type& SparseGraph<node_type, edge_type>::getEdge(int from, int to)const
{
	assert( (from < m_Nodes.size()) &&
		(from >=0)              &&
		m_Nodes[from].getIndex() != invalid_node_index &&
		"<SparseGraph::GetEdge>: invalid 'from' index");

	assert( (to < m_Nodes.size()) &&
		(to >=0)              &&
		m_Nodes[to].getIndex() != invalid_node_index &&
		"<SparseGraph::GetEdge>: invalid 'to' index");

	for (EdgeList::const_iterator curEdge = m_Edges[from].begin();
		curEdge != m_Edges[from].end();
		++curEdge)
	{
		if (curEdge->getDestination() == to) return *curEdge;
	}

	assert (0 && "<SparseGraph::GetEdge>: edge does not exist");
}

//non const version
template <class node_type, class edge_type>
edge_type& SparseGraph<node_type, edge_type>::getEdge(int from, int to)
{
	assert( (from < m_Nodes.size()) &&
		(from >=0)              &&
		m_Nodes[from].getIndex() != invalid_node_index &&
		"<SparseGraph::GetEdge>: invalid 'from' index");

	assert( (to < m_Nodes.size()) &&
		(to >=0)              &&
		m_Nodes[to].getIndex() != invalid_node_index &&
		"<SparseGraph::GetEdge>: invalid 'to' index");

	for (EdgeList::iterator curEdge = m_Edges[from].begin();
		curEdge != m_Edges[from].end();
		++curEdge)
	{
		if (curEdge->getDestination() == to) return *curEdge;
	}

	assert (0 && "<SparseGraph::GetEdge>: edge does not exist");
}

//-------------------------- AddEdge ------------------------------------------
//
//  Use this to add an edge to the graph. The method will ensure that the
//  edge passed as a parameter is valid before adding it to the graph. If the
//  graph is a digraph then a similar edge connecting the nodes in the opposite
//  direction will be automatically added.
//-----------------------------------------------------------------------------
template <class node_type, class edge_type>
void SparseGraph<node_type, edge_type>::addEdge(EdgeType edge)
{
	//first make sure the from and to nodes exist within the graph 
	assert( (edge.getSource() < m_iNextNodeIndex) && (edge.getDestination() < m_iNextNodeIndex) &&
		"<SparseGraph::AddEdge>: invalid node index");

	//make sure both nodes are active before adding the edge
	if ( (m_Nodes[edge.getDestination()].getIndex() != invalid_node_index) && 
		(m_Nodes[edge.getSource()].getIndex() != invalid_node_index))
	{
		//add the edge, first making sure it is unique
		if (UniqueEdge(edge.getSource(), edge.getDestination()))
		{
			m_Edges[edge.getSource()].push_back(edge);
		}

		//if the graph is undirected we must add another connection in the opposite
		//direction
		if (!m_isDigraph)
		{
			//check to make sure the edge is unique before adding
			if (UniqueEdge(edge.getDestination(), edge.getSource()))
			{
				EdgeType NewEdge = edge;

				NewEdge.setDestination(edge.getSource());
				NewEdge.setSource(edge.getDestination());

				m_Edges[edge.getDestination()].push_back(NewEdge);
			}
		}
	}
}


//----------------------------- RemoveEdge ---------------------------------
template <class node_type, class edge_type>
void SparseGraph<node_type, edge_type>::removeEdge(int from, int to)
{
	assert ( (from < (int)m_Nodes.size()) && (to < (int)m_Nodes.size()) &&
		"<SparseGraph::RemoveEdge>:invalid node index");

	EdgeList::iterator curEdge;

	if (!m_bDigraph)
	{
		for (curEdge = m_Edges[to].begin();
			curEdge != m_Edges[to].end();
			++curEdge)
		{
			if (curEdge->getDestination() == from){curEdge = m_Edges[to].erase(curEdge);break;}
		}
	}

	for (curEdge = m_Edges[from].begin();
		curEdge != m_Edges[from].end();
		++curEdge)
	{
		if (curEdge->getDestination() == to){curEdge = m_Edges[from].erase(curEdge);break;}
	}
}

//-------------------------- AddNode -------------------------------------
//
//  Given a node this method first checks to see if the node has been added
//  previously but is now innactive. If it is, it is reactivated.
//
//  If the node has not been added previously, it is checked to make sure its
//  index matches the next node index before being added to the graph
//------------------------------------------------------------------------
template <class node_type, class edge_type>
int SparseGraph<node_type, edge_type>::addNode(node_type node)
{
	if (node.getIndex() < (int)m_Nodes.size())
	{
		//make sure the client is not trying to add a node with the same ID as
		//a currently active node
		assert (m_Nodes[node.getIndex()].getIndex() == invalid_node_index &&
			"<SparseGraph::AddNode>: Attempting to add a node with a duplicate ID");

		m_Nodes[node.getIndex()] = node;

		return m_iNextNodeIndex;
	}

	else
	{
		//make sure the new node has been indexed correctly
		assert (node.getIndex() == m_iNextNodeIndex && "<SparseGraph::AddNode>:invalid index");

		m_Nodes.push_back(node);
		m_Edges.push_back(EdgeList());

		return m_iNextNodeIndex++;
	}
}

//----------------------- CullInvalidEdges ------------------------------------
//
//  iterates through all the edges in the graph and removes any that point
//  to an invalidated node
//-----------------------------------------------------------------------------
template <class node_type, class edge_type>
void SparseGraph<node_type, edge_type>::CullInvalidEdges()
{
	for (EdgeListVector::iterator curEdgeList = m_Edges.begin(); curEdgeList != m_Edges.end(); ++curEdgeList)
	{
		for (EdgeList::iterator curEdge = (*curEdgeList).begin(); curEdge != (*curEdgeList).end(); ++curEdge)
		{
			if (m_Nodes[curEdge->getDestination()].getIndex() == invalid_node_index || 
				m_Nodes[curEdge->getSource()].getIndex() == invalid_node_index)
			{
				curEdge = (*curEdgeList).erase(curEdge);
			}
		}
	}
}

//------------------------------- RemoveNode -----------------------------
//
//  Removes a node from the graph and removes any links to neighbouring
//  nodes
//------------------------------------------------------------------------
template <class node_type, class edge_type>
void SparseGraph<node_type, edge_type>::removeNode(int node)                                   
{
	assert(node < (int)m_Nodes.size() && "<SparseGraph::RemoveNode>: invalid node index");

	//set this node's index to invalid_node_index
	m_Nodes[node].setIndex(invalid_node_index);

	//if the graph is not directed remove all edges leading to this node and then
	//clear the edges leading from the node
	if (!m_isDigraph)
	{    
		//visit each neighbour and erase any edges leading to this node
		for (EdgeList::iterator curEdge = m_Edges[node].begin(); 
			curEdge != m_Edges[node].end();
			++curEdge)
		{
			for (EdgeList::iterator curE = m_Edges[curEdge->getDestination()].begin();
				curE != m_Edges[curEdge->getDestination()].end();
				++curE)
			{
				if (curE->getDestination() == node)
				{
					curE = m_Edges[curEdge->getDestination()].erase(curE);

					break;
				}
			}
		}

		//finally, clear this node's edges
		m_Edges[node].clear();
	}

	//if a digraph remove the edges the slow way
	else
	{
		CullInvalidEdges();
	}
}

//-------------------------- SetEdgeCost ---------------------------------
//
//  Sets the cost of a specific edge
//------------------------------------------------------------------------
template <class node_type, class edge_type>
void SparseGraph<node_type, edge_type>::setEdgeCost(int from, int to, double NewCost)
{
	//make sure the nodes given are valid
	assert( (from < m_Nodes.size()) && (to < m_Nodes.size()) &&
		"<SparseGraph::SetEdgeCost>: invalid index");

	//visit each neighbour and erase any edges leading to this node
	for (EdgeList::iterator curEdge = m_Edges[from].begin(); 
		curEdge != m_Edges[from].end();
		++curEdge)
	{
		if (curEdge->getDestination() == to)
		{
			curEdge->SetCost(NewCost);
			break;
		}
	}
}

//-------------------------------- UniqueEdge ----------------------------
//
//  returns true if the edge is not present in the graph. Used when adding
//  edges to prevent duplication
//------------------------------------------------------------------------
template <class node_type, class edge_type>
bool SparseGraph<node_type, edge_type>::UniqueEdge(int from, int to)const
{
	for (EdgeList::const_iterator curEdge = m_Edges[from].begin();
		curEdge != m_Edges[from].end();
		++curEdge)
	{
		if (curEdge->getDestination() == to)
		{
			return false;
		}
	}

	return true;
}

#endif